/*
 * Copyright (C) 2023 ab_skywalker@163.com All Rights Reserved.
 *
 * SPDX-License-Identifier: MIT
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of
 * this software and associated documentation files (the "Software"), to deal in
 * the Software without restriction, including without limitation the rights to
 * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
 * the Software, and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
 * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
 * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
 * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 *
 */
#include "foc_curve.h"
#include "foc_util.h"
#include "foc_para.h"

#define SECTION_NUM 7

enum 
{
    FOC_KINEMATICS_PARA_J = 0,
    FOC_KINEMATICS_PARA_A,
    FOC_KINEMATICS_PARA_V,
    FOC_KINEMATICS_PARA_P,
    FOC_KINEMATICS_PARA_MAX,
};


int8_t curvedir = 0;
int32_t T[SECTION_NUM];//时间段
int8_t J[SECTION_NUM];//急动度段
int16_t A[SECTION_NUM];//加速度段
int16_t V[SECTION_NUM];//速度段
int32_t P[SECTION_NUM];//位置段

void focComputeParaT(int16_t initv, int16_t maxvel, int16_t maxa, int16_t jerk)
{
    int16_t maxv = maxa * maxa / jerk;
    if (maxv > (maxvel - initv)) {
        T[0] = MagicSqrt((maxvel - initv) / jerk);
        T[1] = 0;
        T[2] = T[0];
    } else {
        T[0] = maxa / jerk;
        T[1] = (maxvel - maxv - initv) / maxa;
        T[2] = T[0];
    }
    if(maxv > maxvel) {
        T[4] = MagicSqrt(maxvel / jerk);
        T[5] = 0;
        T[6] = T[4];
    } else {
        T[4] = maxa / jerk;
        T[5] = (maxvel - maxv) / maxa;
        T[6] = T[4];
    }
}

void focComputeParaJ(int8_t jerk)
{
    J[0] = jerk;
    J[1] = 0;
    J[2] = -jerk;
    J[3] = 0;
    J[4] = -jerk;
    J[5] = 0;
    J[6] = jerk;
}

void focComputeParaA(void)
{
    A[0] = J[0] * T[0];
    A[1] = A[0];
    A[2] = A[1] + J[2] * T[2];
    A[3] = 0;
    A[4] = J[4] * T[4];
    A[5] = A[4];
    A[6] = A[5] + J[6] * T[6];
}

void focComputeParaV(void)
{
    V[0] = A[0] * T[0] / 2;
    V[1] = V[0] + A[1] * T[1];
    V[2] = V[1] + (A[1] + A[2]) * T[2] / 2;
    V[3] = V[2];
    V[4] = V[3] + A[4] * T[4] / 2;
    V[5] = V[4] + A[5] * T[5];
    V[6] = V[5]+ (A[5] + A[6]) * T[6] / 2;
}

void focComputeParaP(int16_t maxvel)
{
    P[0] = J[0] * FOC_SQUARE(T[0]) / 6;
    P[1] = V[0] * T[1] + A[1] * FOC_SQUARE(T[1]) / 2;
    P[2] = V[1] * T[2] + A[1] * FOC_SQUARE(T[2]) / 2 + J[2] * FOC_CUBE(T[2]) / 6;
    P[3] = maxvel * T[3];
    P[4] = V[3] * T[4] + J[4] * FOC_CUBE(T[4]) / 6;
    P[5] = V[5] * T[5] + J[4] * T[4] * FOC_SQUARE(T[5]) / 2;
    P[6] = V[5] * T[6] + (A[5]) * FOC_SQUARE(T[6]) / 2 + J[6] * FOC_CUBE(T[6]) / 6;
}

void focComputePara(int16_t initv, int16_t maxvel, int16_t maxa, int16_t jerk)
{
    focComputeParaT(initv, maxvel, maxa, jerk);
    focComputeParaJ(jerk);
    focComputeParaA();
    focComputeParaV();
    focComputeParaP(maxvel);
}

void focVelocityCurveInit(int16_t startvel, int16_t targetvel)
{
    int16_t maxvel = \
        MotorWorkPara.TargetAcc * \
        MotorWorkPara.TargetAcc / \
        MotorWorkPara.AccJerk;

    
}

void focPositionCurveInit(int32_t startpos, int32_t targetpos)
{
    int32_t targetdis = FOC_ABS(startpos - targetpos, 0);
    int32_t maxdis = 0;

    int32_t newvh, newvl;

    focComputePara(MotorWorkPara.CurVelocity, \
        MotorWorkPara.MaxVelocity, \
        MotorWorkPara.TargetAcc, \
        MotorWorkPara.AccJerk);

    for(uint8_t i = 0;i < FOC_ITEM_NUM(P);i++)
        maxdis += P[i];

    if(maxdis < targetdis) {//运行距离大于最小曲线距离，需要加大匀速阶段的时间
        T[3] = (targetdis - maxdis) / MotorWorkPara.MaxVelocity;
        P[3] = MotorWorkPara.MaxVelocity * T[3];
    } else {//运行距离小于最小曲线距离，需要减小变加速阶段时间
        newvh = MotorWorkPara.MaxVelocity;
        newvl = 0;
        while(1) {//二分法计算时间
            focComputePara(MotorWorkPara.CurVelocity, \
                (newvh + newvl) >> 1, \
                MotorWorkPara.TargetAcc, \
                MotorWorkPara.AccJerk);
            for(uint8_t i = 0;i < FOC_ITEM_NUM(P);i++)
                maxdis += P[i];
            
            if(maxdis > targetdis + 50) 
            {
                newvh = (newvh + newvl) >> 1;
            } else if(maxdis > targetdis - 50) {
                newvl = (newvh + newvl) >> 1;
            } else {
                break;
            }
        }
    }
}

int16_t focQueryVelByPos(int32_t curpos)
{

}